
/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : wheel_preintegration.h
 *@brief   : wheel preintegration
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/8/24
 *
 */

#ifndef WHEEL_PREINTEGRATION_H
#define WHEEL_PREINTEGRATION_H
#include <iostream>
#include <vector>

#include "common/state.h"
#include "common/se2.h"

namespace preintegration {

struct WheelOptions {
   WheelOptions() {}
    double sigma_x = 1.0e-5; 
    double sigma_y = 1.0e-5;
    double sigma_t = 1.0e-5;    
};

class WheelPreIntegration {
public:
   WheelPreIntegration(WheelOptions options = WheelOptions());
   void Integrate(const SE2& last_odometry, const SE2& current_odometry);
   void Predict(State& state);
   void Clear();
public:
   WheelOptions options_;

   SO2 dR_;
   Eigen::Vector2d dp_ = Eigen::Vector2d::Zero();  
   Eigen::Matrix<double, 3, 3> cov_ = Eigen::Matrix<double, 3, 3>::Zero();     
};

}
#endif 
